/*! @file obj_loader.cpp
 *  @brief Utility to load .obj files, containing 3D models of robots.
 *
 *  This is now our own version of obj loader
 *  OBJ files should have a triangular mesh, with normals, but no texture
 * coordinates.
 */

#include "obj_loader.h"

#include <assert.h>
#include <cstdlib>
#include <cstring>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <array>

void load_obj_file(std::string fileName,
                   std::vector<float>& positions,
                   std::vector<float>& normals)
{
    std::ifstream file(fileName, std::ios_base::in);
    std::string line;
    std::vector<std::array<float, 3>> verts;
    std::vector<std::array<float, 3>> vns;
    std::vector<std::array<std::array<int, 2>, 3>> faces;

    while (!file.eof())
    {
        std::getline(file, line);
        std::istringstream iss(line);
        std::string type;
        iss >> type;
        float v0, v1, v2;

        if (type[0] == '#' || type[0] == 'o')
        {
            continue;
        }
        else if (type == "v")
        {
            iss >> v0;
            iss >> v1;
            iss >> v2;
            verts.push_back({v0, v1, v2});
        }
        else if (type == "vn")
        {
            iss >> v0;
            iss >> v1;
            iss >> v2;
            vns.push_back({v0, v1, v2});
        }
        else if (type == "f")
        {
            const char* linePtr = line.c_str() + 2;
            int v, n;

            std::array<std::array<int, 2>, 3> face;
            for (int i = 0; i < 3; i++)
            {
                sscanf(linePtr, "%d//%d", &v, &n);
                face[i][0] = v - 1;
                face[i][1] = n - 1;
                while (linePtr[0] != ' ' && linePtr[0] != 0) ++linePtr;
                while (linePtr[0] == ' ') ++linePtr;
            }
            faces.push_back(face);
        }
        else if (type == "" || type == "s" || type == "g" || type == "mtllib" || type == "usemtl")
        {
            // unhandled, but not an error.
        }
        else
        {
            printf("ERROR bad obj line %s\n", line.c_str());
        }
    }

    positions.resize(9 * faces.size());
    normals.resize(9 * faces.size());

    uint64_t c = 0;
    for (uint64_t i = 0; i < faces.size(); i++)
    {
        for (uint64_t j = 0; j < 3; j++)
        {
            for (uint64_t k = 0; k < 3; k++)
            {
                positions[c] = verts[faces[i][j][0]][k];
                normals[c++] = vns[faces[i][j][1]][k];
            }
        }
    }
}
